/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: TKruse
*********************************************************************/

#include <base_local_planner/obstacle_cost_function.h>
#include <cmath>
#include <Eigen/Core>
#include <ros/console.h>

namespace base_local_planner
{

ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D *costmap)
        : costmap_(costmap), sum_scores_(false)
{
    if (costmap != NULL) world_model_ = new base_local_planner::CostmapModel(*costmap_);
}

ObstacleCostFunction::~ObstacleCostFunction() { delete world_model_; }


void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed)
{
    // TODO: move this to prepare if possible
    max_trans_vel_ = max_trans_vel;
    max_scaling_factor_ = max_scaling_factor;
    scaling_speed_ = scaling_speed;
}
// 获得对代价值进行调节的scale,对轨迹中的每个点进行遍历,检测是否碰撞,计算代价得到f_cost,若sum_scores_为真进行累加,为假选择最大的代价。
// 这里的footprintCost()函数调用的是base_local_planner::WorldModel类中的成员函数footprintCost()计算，
// 得到代价后进行检查不能小于零，小于0，返回-6，不能超出地图范围，超出范围返回-7。
double ObstacleCostFunction::scoreTrajectory(Trajectory &traj)
{
    double cost = 0;
    // 对代价值进行调节
    double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
    double px, py, pth;
    if (footprint_spec_.empty())
    {
        // Bug, should never happen
        ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
        return -9;
    }
    for (unsigned int i = 0; i < traj.getPointsSize(); ++i)
    {
        traj.getPoint(i, px, py, pth);
        double f_cost = footprintCost(px, py, pth, scale, footprint_spec_, costmap_, world_model_);
        if (f_cost < 0) return f_cost;
        if (sum_scores_) cost += f_cost;
        else cost = std::max(cost, f_cost);
    }
    return cost;
}

double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel,
                                              double max_scaling_factor)
{
    double vmag = hypot(traj.xv_, traj.yv_);
    // if we're over a certain speed threshold,
    // we'll scale the robot's footprint to make it either slow down or stay further from walls
    // 如果我们超过了一定的速度阈值，我们将调整机器人的足迹，使其要么减速，要么远离墙壁
    double scale = 1.0;
    if (vmag > scaling_speed)
    {
        // scale up to the max scaling factor linearly... this could be changed later
        // 线性放大到最大比例因子…这可以稍后更改
        double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
        scale = max_scaling_factor * ratio + 1.0;
    }
    return scale;
}

double ObstacleCostFunction::footprintCost(
        const double &x, const double &y, const double &th, double scale,
        std::vector<geometry_msgs::Point> footprint_spec,
        costmap_2d::Costmap2D *costmap,
        base_local_planner::WorldModel *world_model)
{
    std::vector<geometry_msgs::Point> scaled_footprint;
    for (auto & i : footprint_spec)
    {
        geometry_msgs::Point new_pt;
        new_pt.x = scale * i.x;
        new_pt.y = scale * i.y;
        scaled_footprint.push_back(new_pt);
    }
    // check if the footprint is legal
    // TODO: Cache inscribed radius
    double footprint_cost = world_model->footprintCost(x, y, th, scaled_footprint);
    // double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);
    if (footprint_cost < 0) return -6.0;
    unsigned int cell_x, cell_y;
    // we won't allow trajectories that go off the map... shouldn't happen that often anyways
    if (!costmap->worldToMap(x, y, cell_x, cell_y)) return -7.0;
    double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
    return occ_cost;
}

} /* namespace base_local_planner */
